﻿#region File Description
//-----------------------------------------------------------------------------
// Game.cs
//
// Microsoft XNA Community Game Platform
// Copyright (C) Microsoft Corporation. All rights reserved.
//-----------------------------------------------------------------------------
#endregion

using SynapseGaming.SunBurn.Framework.Primitives;
using System;
using System.Collections.Generic;
using System.Text;

namespace GameLibrary
{
    /// <summary>
    /// Represents a simple triangle by the vertices at each corner.
    /// </summary>
    public struct Triangle
    {
        public Vector3 V0;
        public Vector3 V1;
        public Vector3 V2;

        public Vector3 Normal;

        public Triangle(Vector3 v0, Vector3 v1, Vector3 v2)
        {
            V0 = v0;
            V1 = v1;
            V2 = v2;

            Vector3 edge1 = v1 - v0;
            Vector3 edge2 = v0 - v2;

            Normal = Vector3.Cross(edge1, edge2);
            Normal.Normalize();
        }
    }

    public static class CollisionHelper
    {
        public static float? RayIntersectsModel(Vector3[] vertices, Matrix sceneTransform, Ray pickRay)
        {
            Matrix inverseSceneTransform = Matrix.Invert(sceneTransform);

            Vector3 rayPosition = Vector3.Transform(pickRay.Position,
                                                    inverseSceneTransform);

            Vector3 rayDirection = Vector3.TransformNormal(pickRay.Direction,
                                                           inverseSceneTransform);

            pickRay.Position = rayPosition;
            pickRay.Direction = rayDirection;

            float? closestIntersection = null;

            for (int i = 0; i < vertices.Length; i += 3)
            {
                float? intersection = null;

                if (i + 1 < vertices.Length && i + 2 < vertices.Length)
                {
                    RayIntersectsTriangle(ref pickRay,
                                          ref vertices[i],
                                          ref vertices[i + 1],
                                          ref vertices[i + 2],
                                          out intersection);
                }

                if (intersection != null)
                {
                    if (closestIntersection == null || intersection < closestIntersection)
                        closestIntersection = intersection;
                }
            }

            return closestIntersection;
        }

        public static float? RayIntersectsModel(Vector3[] vertices, BoundingBox bounds, Matrix sceneTransform, Ray pickRay)
        {
            if (pickRay.Intersects(bounds) == null)
                return null;

            else
            {
                Matrix inverseSceneTransform = Matrix.Invert(sceneTransform);

                Vector3 rayPosition = Vector3.Transform(pickRay.Position,
                                                        inverseSceneTransform);

                Vector3 rayDirection = Vector3.TransformNormal(pickRay.Direction,
                                                               inverseSceneTransform);

                pickRay.Position = rayPosition;
                pickRay.Direction = rayDirection;

                float? closestIntersection = null;

                for (int i = 0; i < vertices.Length; i += 3)
                {
                    float? intersection = null;

                    if (i + 1 < vertices.Length && i + 2 < vertices.Length)
                    {
                        RayIntersectsTriangle(ref pickRay,
                                              ref vertices[i],
                                              ref vertices[i + 1],
                                              ref vertices[i + 2],
                                              out intersection);
                    }

                    if (intersection != null)
                    {
                        if ((closestIntersection == null) ||
                            (intersection < closestIntersection))
                        {
                            closestIntersection = intersection;
                        }
                    }
                }

                return closestIntersection;
            }
        }

        public static void RayIntersectsTriangle(ref Ray ray,
                                  ref Vector3 vertex1,
                                  ref Vector3 vertex2,
                                  ref Vector3 vertex3, out float? result)
        {
            // Compute vectors along two edges of the triangle.
            Vector3 edge1, edge2;

            Vector3.Subtract(ref vertex2, ref vertex1, out edge1);
            Vector3.Subtract(ref vertex3, ref vertex1, out edge2);

            // Compute the determinant.
            Vector3 directionCrossEdge2;
            Vector3.Cross(ref ray.Direction, ref edge2, out directionCrossEdge2);

            float determinant;
            Vector3.Dot(ref edge1, ref directionCrossEdge2, out determinant);

            // If the ray is parallel to the triangle plane, there is no collision.
            if (determinant > -float.Epsilon && determinant < float.Epsilon)
            {
                result = null;
                return;
            }

            // If CullBackFaces And det < 0 Then Exit Function
            //if (determinant > 0)
            //{
            //    result = null;
            //    return;
            //}

            float inverseDeterminant = 1.0f / determinant;

            // Calculate the U parameter of the intersection point.
            Vector3 distanceVector;
            Vector3.Subtract(ref ray.Position, ref vertex1, out distanceVector);

            float triangleU;
            Vector3.Dot(ref distanceVector, ref directionCrossEdge2, out triangleU);
            triangleU *= inverseDeterminant;

            // Make sure it is inside the triangle.
            if (triangleU < 0 || triangleU > 1)
            {
                result = null;
                return;
            }

            // Calculate the V parameter of the intersection point.
            Vector3 distanceCrossEdge1;
            Vector3.Cross(ref distanceVector, ref edge1, out distanceCrossEdge1);

            float triangleV;
            Vector3.Dot(ref ray.Direction, ref distanceCrossEdge1, out triangleV);
            triangleV *= inverseDeterminant;

            // Make sure it is inside the triangle.
            if (triangleV < 0 || triangleU + triangleV > 1)
            {
                result = null;
                return;
            }

            // Compute the distance along the ray to the triangle.
            float rayDistance;
            Vector3.Dot(ref edge2, ref distanceCrossEdge1, out rayDistance);
            rayDistance *= inverseDeterminant;

            // Is the triangle behind the ray origin?
            if (rayDistance < 0)
            {
                result = null;
                return;
            }

            result = rayDistance;
        }
    }
}
